using System;
using Microsoft.SPOT;
using NetduinoHappyHour.Hardware;
using NetduinoHappyHour.Util;
using SecretLabs.NETMF.Hardware;
using SecretLabs.NETMF.Hardware.Netduino;
using System.Collections;
using NetduinoHappyHour.Asynchronous;

namespace NetduinoHappyHour.Behaviour
{
    enum Evasion
    {
        Soft,
        Hard,
        Turn,
        Random,
        None
    }

    /// <summary>
    /// makes the robot strolling around, triing not to bump against any objects
    /// </summary>
    public class ExploreByUltrasound : BaseBehaviour, IExecutable
    {

        public void Execute()
        {
            bool debug = false;
            DriveCommand driveStraight = new DriveCommand(200, 200, SystemConfig.Instance().DelayStepper);
            //DisplayMessage message = new DisplayMessage("");
            ISensor ultrasonicSensor = SensorFactory.Instance("JohnWilson.HC_SR04", Pins.GPIO_PIN_D3, Pins.GPIO_PIN_D2);//new JohnWilson.HC_SR04(Pins.GPIO_PIN_D2, Pins.GPIO_PIN_D3);
            double objectAhead = 0;
            bool runExplorer = true;
            Evasion evade = Evasion.None;

            driveStraight.SelfRepeating = true;
            ExecuteAsync execAsync = new ExecuteAsync(driveStraight);
            execAsync.Execute();
            while (runExplorer)
            {
                objectAhead = ultrasonicSensor.GetDistance();
                if (objectAhead > 0 && objectAhead < 5)
                {
                    evade = Evasion.Turn;
                }
                else if (objectAhead > 0 && objectAhead < 10)
                {
                    evade = Evasion.Hard;
                }
                else if (objectAhead > 0 && objectAhead < 20)
                {
                    evade = Evasion.Soft;
                }

                if (evade != Evasion.None)
                {
                    if (debug)
                    {
                        SystemStatus.Instance().AddMessage("Maneuver" + evade);
                    }

                    driveStraight.IsValid = false;
                    execAsync.Stop();
                    driveStraight.Reset();
                    // evasion maneuver
                    gw.ExecuteDriveCommand(GetEvasionManeuver(evade));
                    evade = Evasion.None;
                    // start from beginning
                    execAsync = new ExecuteAsync(driveStraight);
                    execAsync.Execute();
                }
            }
        }

        /// <summary>
        /// TODO improve avoiding
        /// </summary>
        /// <param name="sensorData">Input Data to create appropriate evasion maneuver</param>
        /// <returns></returns>
        private DriveCommand GetEvasionManeuver(Evasion howTo)
        {
            DriveCommand evasionCommand = null;
            switch (howTo)
            {
                case Evasion.Turn:
                    evasionCommand = new DriveCommand(-SystemConfig.Instance().Turn90 * 2, -SystemConfig.Instance().Turn90 * 2, SystemConfig.Instance().DelayStepper);
                    break;
                case Evasion.Hard:
                    evasionCommand = new DriveCommand(-SystemConfig.Instance().Turn90, SystemConfig.Instance().Turn90, SystemConfig.Instance().DelayStepper);
                    break;
                case Evasion.Soft:
                default:
                    evasionCommand = new DriveCommand(-SystemConfig.Instance().Rotate90 / 2, SystemConfig.Instance().Rotate90 / 2, SystemConfig.Instance().DelayStepper);
                    break;
            }
            return evasionCommand;
        }
    }
}
